/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvWorld/Src/bezier_path.cpp
 * @Description  : 实现了贝塞尔路径类的功能，包括路径的初始化、控制点的获取、方向角的计算以及贝塞尔轨迹的生成。 
 *                 BezierPath类提供了构造函数用于创建路径对象，并包含方法来检查模糊节点限制、获取方向角、判断路径类型等。
 *                 路径的创建和初始化过程也在此类中完成，确保路径的正确性和有效性。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:38:07
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include <stdlib.h>
#include "tools.h"
#include "path.h" 
//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "CBezierPath".
//
BezierPath::BezierPath() : is_shift_(0) {}

/**
 * @brief BezierPath 构造函数
 *
 * 初始化 BezierPath 对象，包含贝塞尔曲线的起点、终点和控制点信息
 *
 * @param id 路径的唯一标识符
 * @param start_node 路径的起始节点
 * @param end_node 路径的结束节点
 * @param ctrl_point1 第一个控制点
 * @param ctrl_point2 第二个控制点
 * @param velocity_limit 路径的速度限制
 * @param guide_type 引导类型
 * @param move_dir 移动方向标签
 */
BezierPath::BezierPath(USHORT id, USHORT start_node, USHORT end_node, const Point& ctrl_point1, const Point& ctrl_point2,
                       float velocity_limit, USHORT guide_type, MoveDirTag move_dir) 
    : Path(id, start_node, end_node, velocity_limit, move_dir, PathType::BEZIER_TYPE, guide_type) {
    ctrl_points_[0] = ctrl_point1;
    ctrl_points_[1] = ctrl_point2;
    Init();
}

/**
 * @brief 获取第一个控制点
 *
 * 返回贝塞尔路径中的第一个控制点。
 *
 * @return 返回一个对第一个控制点的常量引用
 */
const Point& BezierPath::GetCtrlPoint1() const 
{
    return ctrl_points_[0];
}

const Point& BezierPath::GetCtrlPoint2() const 
{
    return ctrl_points_[1];
}

// CAngle &CBezierPath::GetShiftHeading()
// {
//     return m_angShiftHeading;
// }
/**
 * @brief 检查贝塞尔路径模糊节点的限制值
 *
 * 此函数返回一个浮点数，表示贝塞尔路径模糊节点的限制值。
 *
 * @return 返回模糊节点的限制值，默认为0.06f。
 */
float BezierPath::FuzzyNodeCheckLimit() const 
{
    return 0.06f;
}


/**
 * @brief 初始化 BezierPath 对象
 *
 * 此函数用于初始化 BezierPath 对象，包括计算起始和结束的方向角度，以及 Bezier 轨迹的总长度。
 *
 * 如果 BezierPath 对象被设置为平移模式（is_shift_ 为 true），则起始和结束的方向角度都设置为指定的角度（ang_head_）。
 * 否则，起始和结束的方向角度将基于起始点和控制点之间的连线斜率计算。
 * 如果移动方向（move_dir_）为反向，则起始和结束的方向角度将取反。
 *
 * 最后，通过动态类型转换为 BezierTraj 类型并调用其 GetTotalLength 方法来计算 Bezier 轨迹的总长度，
 * 并将其存储在 size_ 成员变量中。如果转换失败，则 size_ 被设置为 -1。
 */
void BezierPath::Init() 
{
    Point start_point = GetStartPoint();
    Point end_point = GetEndPoint();
    Line start_line(start_point, ctrl_points_[0]);
    Line end_line(ctrl_points_[1], end_point);

    if (is_shift_) {
        start_heading_ = end_heading_ = heading_angle_ = Angle(ang_head_);
    } else {
        start_heading_ = start_line.SlantAngle();
        end_heading_ = end_line.SlantAngle();
        start_heading_ = move_dir_ == MoveDirTag::Backward ? !start_heading_ : start_heading_;
        end_heading_ = move_dir_ == MoveDirTag::Backward ? !end_heading_ : end_heading_;
    }

    BezierTrajectory* temp_bezier = dynamic_cast<BezierTrajectory*>(MakeTraj());
    if (temp_bezier) {
        size_ = temp_bezier->GetTotalLength();
        delete temp_bezier;
    } else {
        size_ = -1;
    }
}

/**
 * @brief 获取给定节点的方向角
 *
 * 根据传入的节点获取对应的方向角。
 *
 * @param node 要获取方向角的节点
 * @return 返回给定节点的方向角引用
 */
Angle& BezierPath::GetHeading(const Node node) 
{
    if (node == start_node_) {
        return start_heading_;
    } else if (node == end_node_) {
        return end_heading_;
    } else {
        return heading_angle_;
    }
}

/**
 * @brief 判断路径是否是侧移路径
 *
 * @return 如果是侧移路径，则返回 true；否则返回 false
 */
bool BezierPath::IsShift() const 
{
    return is_shift_;
}

/**
 * @brief 生成贝塞尔轨迹
 *
 * 生成一个由起点、两个控制点和终点定义的贝塞尔轨迹。
 *
 * @return 返回生成的贝塞尔轨迹对象指针
 */
Trajectory* BezierPath::MakeTraj() 
{
    BezierTrajectory* bezier_trajectory = new BezierTrajectory;
    Point start_point = GetStartPoint();
    Point end_point = GetEndPoint();
    bezier_trajectory->CreateTrajectory(start_point, ctrl_points_[0], ctrl_points_[1], end_point, move_dir_, is_shift_, heading_angle_);
    return bezier_trajectory;
}

/**
 * @brief 创建贝塞尔路径
 *
 * 该函数用于初始化并创建一个贝塞尔路径。
 *
 * @return 如果创建成功，返回 true；否则返回 false。
 */
bool BezierPath::Create() 
{
    Init();
    return true;
}
